/*!
 * GEOCalc 实现了基本的WGS-84坐标系的计算。
 * by CEStdio
 * 1997-2023
 *
*/
#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <vector>
#include <functional>

namespace CES_GEOCALC {

// 定义地球长半轴
inline const double w_a = 6378137;
// 定义圆周率
inline const double w_pi = 3.14159265358979323846;
// 定义地球扁率
inline const double w_f = 1.0 / 298.257223563;
// 定义地球短半轴
inline const double w_b = (1-w_f)*w_a;
// 定义地球第一偏心率的平方
inline const double w_e = sqrt(w_f * (2 - w_f));
// 定义地球第二偏心率的平方
inline const double w_e2 = w_f * (2 - w_f);
// 定义角度转弧度的系数
inline const double deg2rad = w_pi / 180.0;
// 定义弧度转角度的系数
inline const double rad2deg = 180.0 / w_pi;


//! 1. 基础工具函数区
//////////////////////////////////////////////////////////////////////////////
// 定义一个内联函数，用于计算一个三维向量的最大坐标绝对值
inline double vec_max_coordabs(const double x[])
{
	// 计算第一个坐标的绝对值
	double r1 = x[0]<0?-x[0]:x[0];
	// 计算第二个坐标的绝对值
	double r2 = x[1]<0?-x[1]:x[1];
	// 计算第三个坐标的绝对值
	double r3 = x[2]<0?-x[2]:x[2];
	// 初始化最大值为第一个坐标的绝对值
	double r = r1;
	// 如果第二个坐标的绝对值大于最大值，则更新最大值
	if (r < r2) r = r2;
	// 如果第三个坐标的绝对值大于最大值，则更新最大值
	if (r < r3) r = r3;
	// 返回最大值
	return r;
}


/*!
 * \brief vec_distance 计算向量的直线距离
 * \param x1	向量1
 * \param x2	向量2
 * \return  直线距离
 */
// 计算两个三维向量的欧氏距离
inline double vec_distance(const double x1[],const double x2[])
{
	// 计算两个向量的差值的平方和
	return sqrt(
		(x1[0]-x2[0])*(x1[0]-x2[0])+ // x轴方向的差值的平方
		(x1[1]-x2[1])*(x1[1]-x2[1])+ // y轴方向的差值的平方
		(x1[2]-x2[2])*(x1[2]-x2[2])  // z轴方向的差值的平方
		);
}

// 定义一个内联函数，用于计算三维向量的模
inline double vec_abs(const double x[])
{
	// 计算向量的模，即向量的长度
	return sqrt(
		x[0]*x[0]+x[1]*x[1]+x[2]*x[2]
		);
}

// 定义一个内联函数，用于计算两个向量的叉积
inline void vec_cross(const double x[],const double y[],double z[])
{
	// 计算叉积的第一个分量
	z[0] = x[1]*y[2] - x[2]*y[1];
	// 计算叉积的第二个分量
	z[1] = x[2]*y[0] - x[0]*y[2];
	// 计算叉积的第三个分量
	z[2] = x[0]*y[1] - x[1]*y[0];
}


inline double vec_dot(const double x1[],const double x2[])
{
	return ((x2[0]*x1[0])+(x2[1]*x1[1])+(x2[2]*x1[2]));
}
/*!
 * \brief vec_rotation 罗德里格旋转公式（Rodrigues' rotation formula）
 * \param v		旋转前向量
 * \param k		旋转轴
 * \param sita  转角
 * \param res	旋转后向量
 * \param rad 弧度true/度 false
 */
inline void vec_rotation(const double v[3],const double k[3],const double dsita, double res[3],const bool rad = false)
{
	//res = v cos(sita) + (1-cos(sita)).(k.v).k + k x v sin(sita);
	//sita为弧度制或角度制，默认为角度制
	const double sita = rad?dsita:(deg2rad*dsita);
	//计算cos(sita)
	const double cos_sita = cos(sita);
	//计算sin(sita)
	const double sin_sita = sin(sita);
	//计算k.v
	const double kdv = vec_dot(k,v);
	//计算k x v
	double kcv[3] = {0,0,0};
	vec_cross(k,v,kcv);

	//计算(1-cos(sita)).(k.v)
	const double md = (1-cos_sita)* kdv;

	//计算res
	res[0] = v[0] * cos_sita + md * k[0] + kcv[0] * sin_sita;
	res[1] = v[1] * cos_sita + md * k[1] + kcv[1] * sin_sita;
	res[2] = v[2] * cos_sita + md * k[2] + kcv[2] * sin_sita;
}

// 定义一个内联函数，用于计算两个参数的反正切值
inline double atanCal(const double ppa,const double ppb)
{
	// 使用atan2函数计算反正切值
	double v1 = atan2(ppa,ppb);
	// 如果计算结果小于0，则加上2*pi
	if (v1 <0)
		v1 += 2*w_pi;
	// 返回计算结果
	return v1;
}

//! 2. 基础ECEF LLA ENU 转换
//////////////////////////////////////////////////////////////////////////////


/*!
 * \brief lla2ecef 经纬度坐标到ECEF，
 * \param lla  纬经高(默认)/经纬高, 量纲是度(默认)/弧度、米
 * \param ecef xyz，量纲是米
 * \param pr 传出正球半径r,量纲是米
 * \param pd 传出正球原点偏移 OO' d 量纲是米
 * \param rad  角度量纲开关，false 是度，true 是弧度
 * \param latfirst 经纬度顺序，false 是经度\纬度\高度，true 是纬度\经度\高度
*/
// 将经纬度转换为地球坐标
inline void lla2ecef(
	// 输入经纬度数组
	const double lla[/*3*/],
	// 输出地球坐标数组
	double ecef[/*3*/],
	// 输出地球半径
	double * pr = nullptr,
	// 输出地球曲率
	double * pd = nullptr,
	const bool rad = false,
	const bool latfirst = true
	)
{
	const double lat = latfirst ? (rad ? (lla[0]) : (lla[0] * deg2rad))
								: (rad ? (lla[1]) : (lla[1] * deg2rad));
	const double lon = latfirst ? (rad ? (lla[1]) : (lla[1] * deg2rad))
								: (rad ? (lla[0]) : (lla[0] * deg2rad));
	const double H = lla[2];
	const double r = w_a / sqrt(1 - w_e2 * sin(lat) * sin(lat));
	const double d = - r * w_e2 * sin(lat);
	ecef[0] = (r + H) * cos(lat) * cos(lon);
	ecef[1] = (r + H) * cos(lat) * sin(lon);
	ecef[2] = (r + H) * sin(lat) + d;

	if (pr) *pr = r;
	if (pd) *pd = d;
}

/*!
 * \brief ecef2lla ECEF到经纬度坐标
 * \param ecef xyz，量纲是米
 * \param lla  纬经高(默认)/经纬高, 量纲是度(默认)/弧度、米
 * \param pr 传出正球半径r,量纲是米
 * \param pd 传出正球原点偏移 OO' d 量纲是米
 * \param maxiter  最大迭代次数
 * \param piter  迭代次数输出，可以为null
 * \param eps  Z误差门限
 * \param rad  角度量纲开关，false 是度，true 是弧度
 * \param latfirst 经纬度顺序，false 是经度\纬度\高度，true 是纬度\经度\高度
 * \return 迭代收敛标志
*/
inline bool ecef2lla(
	const double ecef[/*3*/],
	double lla[/*3*/],
	double * pr = nullptr,
	double * pd = nullptr,
	const int maxiter = 32,
	int * piter = nullptr,
	const double eps = 1e-10,
	const bool rad = false,
	const bool latfirst = true
	)
{
	const double x = ecef[0], y = ecef[1], z = ecef[2];
	double d = 0;
	double err = 0;
	int iter = 0;
	double Rp = 0, sinphi = 0, rp = 0, dp = 0, zp = 0;
	do {
		Rp = sqrt(x * x + y * y + (z - d) * (z - d));
		sinphi = (z - d) / Rp;
		rp = w_a / sqrt(1 - w_e2 * sinphi * sinphi);
		dp = - rp * w_e2 * sinphi;
		zp = Rp * sinphi + dp;
		d = dp;
		err = z < zp?zp-z:z-zp;
		++iter;
	} while (err > eps && iter < maxiter);

	const double lat = asin(sinphi);
	const double lon = atan2(y, x);
	const double H = Rp - rp;

	lla[latfirst?0:1] = lat * (rad?1:rad2deg);
	lla[latfirst?1:0] = lon * (rad?1:rad2deg);
	lla[2] = H;

	if (piter) *piter = iter;
	if (pr) *pr = rp;
	if (pd) *pd = d;

	return err <= eps;
}

/*!
 * \brief enu_R_from_lla 计算东北天ENU坐标系的旋转矩阵R
 * \param lla 原点经纬度
 * \param R   R为行向量
 * \param rad 角度量纲开关，false 是度，true 是弧度
 * \param latfirst 经纬度顺序，false 是经度\纬度\高度，true 是纬度\经度\高度
 */
inline void enu_R_from_lla(
	const double lla[/*3*/],
	double R[/*3*/][3],
	const bool rad = false,
	const bool latfirst = true
	)
{
	const double fine = latfirst ? (rad ? (lla[0]) : (lla[0] * deg2rad))
								 : (rad ? (lla[1]) : (lla[1] * deg2rad));
	const double sita = latfirst ? (rad ? (lla[1]) : (lla[1] * deg2rad))
								 : (rad ? (lla[0]) : (lla[0] * deg2rad));
	const double cos_sita = cos(sita);
	const double sin_sita = sin(sita);
	const double cos_fine = cos(fine);
	const double sin_fine = sin(fine);

	R[0][0] = -sin_sita;
	R[0][1] = cos_sita;
	R[0][2] = 0;

	R[1][0] = -sin_fine*cos_sita;
	R[1][1] = -sin_fine*sin_sita;
	R[1][2] = cos_fine;

	R[2][0] = cos_fine*cos_sita;
	R[2][1] = cos_fine*sin_sita;
	R[2][2] = sin_fine;
}

/*!
 * \brief ecef2enu	从ECEF到东北天ENU坐标系，AB不重合
 * \param R_A		原点A的旋转阵，通过enu_R_from_lla计算
 * \param A_ecef	原点A的ECEF
 * \param B_ecef	B的ECEF
 * \param B_enu		B的东北天ENU坐标
 */
inline void ecef2enu(
	const double R_A	[/*3*/][3],
	const double A_ecef	[/*3*/],
	const double B_ecef	[/*3*/],
	double B_enu		[/*3*/]
	)
{
	const double B_A[] = {
		B_ecef[0]-A_ecef[0],
		B_ecef[1]-A_ecef[1],
		B_ecef[2]-A_ecef[2],
	};

	B_enu[0] = R_A[0][0] * B_A[0] + R_A[0][1] * B_A[1] + R_A[0][2] * B_A[2];
	B_enu[1] = R_A[1][0] * B_A[0] + R_A[1][1] * B_A[1] + R_A[1][2] * B_A[2];
	B_enu[2] = R_A[2][0] * B_A[0] + R_A[2][1] * B_A[1] + R_A[2][2] * B_A[2];
}

/*!
 * \brief ecef2enu	从ECEF到东北天ENU坐标系，AB重合
 * \param R_A		原点A的旋转阵，通过enu_R_from_lla计算
 * \param B_ecef	B的ECEF指向
 * \param B_enu		B的东北天ENU坐标z指向。
 */
inline void ecef2enu(
	const double R_A	[/*3*/][3],
	const double B_A	[/*3*/],
	double B_enu		[/*3*/]
	)
{
	B_enu[0] = R_A[0][0] * B_A[0] + R_A[0][1] * B_A[1] + R_A[0][2] * B_A[2];
	B_enu[1] = R_A[1][0] * B_A[0] + R_A[1][1] * B_A[1] + R_A[1][2] * B_A[2];
	B_enu[2] = R_A[2][0] * B_A[0] + R_A[2][1] * B_A[1] + R_A[2][2] * B_A[2];
}

/*!
 * \brief enu2ecef	从东北天ENU坐标系到ECEF，AB不重合
 * \param R_A		原点A的旋转阵，通过enu_R_from_lla计算
 * \param A_ecef	原点A的ECEF
 * \param B_enu		B的东北天ENU坐标
 * \param B_ecef	B的ECEF
 */
inline void enu2ecef(
	const double R_A	[/*3*/][3],
	const double A_ecef	[/*3*/],
	const double B_enu	[/*3*/],
	double B_ecef		[/*3*/]
	)
{
	B_ecef[0] =  R_A[0][0] * B_enu[0] + R_A[1][0] * B_enu[1] + R_A[2][0] * B_enu[2] + A_ecef[0] ;
	B_ecef[1] =  R_A[0][1] * B_enu[0] + R_A[1][1] * B_enu[1] + R_A[2][1] * B_enu[2] + A_ecef[1] ;
	B_ecef[2] =  R_A[0][2] * B_enu[0] + R_A[1][2] * B_enu[1] + R_A[2][2] * B_enu[2] + A_ecef[2] ;
}

/*!
 * \brief enu2ecef	从东北天ENU坐标系到ECEF，AB重合，用于旋转
 * \param R_A		原点A的旋转阵，通过enu_R_from_lla计算
 * \param B_enu		B的东北天ENU坐标
 * \param B_ecef	B的ECEF
 */
inline void enu2ecef(
	const double R_A	[/*3*/][3],
	const double B_enu	[/*3*/],
	double B_ecef		[/*3*/]
	)
{
	B_ecef[0] =  R_A[0][0] * B_enu[0] + R_A[1][0] * B_enu[1] + R_A[2][0] * B_enu[2]  ;
	B_ecef[1] =  R_A[0][1] * B_enu[0] + R_A[1][1] * B_enu[1] + R_A[2][1] * B_enu[2]  ;
	B_ecef[2] =  R_A[0][2] * B_enu[0] + R_A[1][2] * B_enu[1] + R_A[2][2] * B_enu[2]  ;
}

/*!
 * \brief ellipsoid_intersections 求取过A的三维斜率为k的直线与椭球的交点。
 * \param A_ecef  A点的ECEF坐标
 * \param k  直线的单位矢量方向
 * \param H  椭球海拔
 * \param result  交点
 * \param pt  截距，即 A + kt 的 t
 * \return 交点个数。
 */
inline int ellipsoid_intersections(
	const double A_ecef[3],
	const double k[3],
	const double H,
	double result[/*2*/][3],
	double *pt/*2*/ = nullptr)
{
	//为了降低计算误差，全部根据A的尺度缩小尺度。
	const double dc = 1.0/vec_max_coordabs(A_ecef);
	const double A = A_ecef[0] * dc;
	const double B = k[0];
	const double C = A_ecef[1] * dc;
	const double D = k[1];
	const double E = A_ecef[2] * dc;
	const double F = k[2];
	const double G = 1/(w_a* dc+H* dc)/(w_a* dc+H* dc);
	const double K = 1/(w_b* dc+H* dc)/(w_b* dc+H* dc);

	//! - A^2 D^2 G^2 - A^2 F^2 G K + 2 A B C D G^2 + 2 A B E F G K
	//! - B^2 C^2 G^2 - B^2 E^2 G K + B^2 G - C^2 F^2 G K
	//! + 2 C D E F G K - D^2	E^2 G K + D^2 G + F^2 K
	const double delta =
		-A*A*D*D*G*G -A*A*F*F*G*K +2*A*B*C*D*G*G +2*A*B*E*F*G*K
		-B*B*C*C*G*G -B*B*E*E*G*K +B*B*G -C*C*F*F*G*K
		+2*C*D*E*F*G*K - D*D*E*E*G*K + D*D*G + F*F*K
		;
	if (delta <0)
		return 0;
	const double sdelta = sqrt(delta);
	//!- \frac
	//! {A B G + C D G + E F K}
	//! {B^2 G + D^2 G + F^2 K}
	const double V1 = -(A*B*G+C*D*G+E*F*K)/(B*B*G+D*D*G+F*F*K);
	const double V2 = 1/(B*B*G+D*D*G+F*F*K);
	int nr  = 0;
	if (sdelta < dc/100)
	{
		const double Vt = V1;
		result[0][0] = (A + B * Vt)/dc;
		result[0][1] = (C + D * Vt)/dc;
		result[0][2] = (E + F * Vt)/dc;
		if (pt)
			pt[0]=Vt /dc;
		nr = 1;
	}
	else
	{
		const double Vt[2] = {V1 + sdelta * V2,V1 - sdelta * V2};
		//由近及远摆放
		int ia = (fabs(Vt[0])<fabs(Vt[1]))?0:1;
		result[0][0] = (A + B * Vt[ia])/dc;
		result[0][1] = (C + D * Vt[ia])/dc;
		result[0][2] = (E + F * Vt[ia])/dc;
		if (pt)
			pt[0]=Vt[ia] /dc;
		result[1][0] = (A + B * Vt[1-ia])/dc;
		result[1][1] = (C + D * Vt[1-ia])/dc;
		result[1][2] = (E + F * Vt[1-ia])/dc;
		if (pt)
			pt[1]=Vt[1-ia] /dc;
		nr = 2;
	}
	return nr;
}

//! 3. 椭球测地线
//////////////////////////////////////////////////////////////////////////////


/*!
 * \brief ellips_geodesic	椭球的最短测地线求取
 * \param llaA_in			A点经纬度
 * \param llaB_in			B点经纬度
 * \param maxResults		结果的最大长度
 * \param resultSpan		结果的间隔为多个步长
 * \param res_lla			存储经纬度的结果
 * \param res_ecef			存储ECEF的结果
 * \param res_Distance		存储距离的结果
 * \param res_Direction		存储方向的结果
 * \param res_err			存储剩余距离的结果
 * \param rad				经纬度为弧度
 * \param latfirst			纬度在前
 * \param enudelta			最大步长
 * \return					结果个数
 */
inline int ellips_geodesic(

	//输入参数
	const double llaA_in[/*3*/], //起始点经纬度高度
	const double llaB_in[/*3*/], //终点经纬度高度
	const int maxResults, //最大结果数
	const int resultSpan=1, //结果间隔
	double (*res_lla)[3] = nullptr, //结果经纬度
	double (*res_ecef)[3] = nullptr, //结果ECEF坐标
	double (*res_Distance) = nullptr, //结果距离
	double (*res_Direction) = nullptr, //结果方向
	double (*res_err) = nullptr, //结果误差
	const bool rad = false, //是否为弧度
	const bool latfirst = true, //是否为纬度在前
	const double enudelta = 1000 //ENU坐标增量
	)
{
	int res = 0; //结果计数
	const double H = (llaA_in[2]+llaB_in[2])/2; //起始点和终点的平均高度
	double llaA[] = {llaA_in[0],llaA_in[1],H}; //起始点经纬度高度
	double llaB[] = {llaB_in[0],llaB_in[1],H}; //终点经纬度高度
	double D = 0; //距离

	double A_ECEF[] = {0,0,0}, B_ECEF[] = {0,0,0}; //起始点和终点的ECEF坐标
	//计算B的ECEF
	lla2ecef(llaB,B_ECEF,nullptr,nullptr,rad,latfirst);
	double err = 0; //误差
	int cnt = 0; //计数
	double dlft_last = 1e99; //剩余距离
	do{
		double A_r = 0, A_d = 0; //起始点的半径和高度
		//计算A的ECEF
		lla2ecef(llaA,A_ECEF,&A_r,&A_d,rad,latfirst);
		//剩余距离
		const double dlft = vec_distance(A_ECEF,B_ECEF);
		if (dlft > dlft_last )
			break;
		dlft_last = dlft;
		//确定本轮 delta
		const double delta = enudelta<dlft?(enudelta):(dlft);

		double R_A[3][3];
		enu_R_from_lla(llaA,R_A); //计算A的ENU坐标旋转矩阵
		//计算B的ENU坐标
		double B_ENU[] = {0,0,0};
		ecef2enu(R_A,A_ECEF,B_ECEF,B_ENU);

		//计算当前方向
		double C = atanCal(B_ENU[0],B_ENU[1]);
		const double C_enu[] = {sin(C),cos(C),0};
		const double Oz [] = {0,0,0};
		//单位矢量 Cs = C
		double Cs[] = {0,0,0};
		enu2ecef(R_A,Oz,C_enu,Cs);


		//推进步进
		const double As[] = {A_ECEF[0],A_ECEF[1],A_ECEF[2]-A_d};
		const double omega = delta/(A_r+H);
		const double cos_omega = cos(omega);
		const double sin_omega = sin(omega);

		//求取 mu
		double mu[3] = {0,0,0};
		vec_cross(As,Cs,mu);
		const double las = vec_abs(As);
		mu[0] /= las;
		mu[1] /= las;
		mu[2] /= las;

		//mu x As
		double muCAs[3] = {0,0,0};
		vec_cross(mu,As,muCAs);

		//A's using 罗德里格旋转公式（Rodrigues' rotation formula）
		const double Ap_s[] = {
			cos_omega * As[0] + sin_omega * muCAs[0],
			cos_omega * As[1] + sin_omega * muCAs[1],
			cos_omega * As[2] + sin_omega * muCAs[2],
		};



		//计算下一位置
		double Ap_ECEF[] = {Ap_s[0],Ap_s[1],Ap_s[2]+A_d};
		double Ap_lla[] = {0,0,0};
		//转为经纬度、修正高度
		double Ap_r = 0, Ap_d = 0;
		int iter = 0;
		ecef2lla(Ap_ECEF,Ap_lla,
				 &Ap_r,&Ap_d,
				 32,&iter,1e-10,
				 rad,latfirst);

		Ap_lla[2] = H;

		//距离累加
		D += (Ap_r + A_r+2*H)/2*omega;

		//终止条件
		err = dlft;

		//输出中间路径
		if(res < maxResults && (cnt % resultSpan==0||err<=1))
		{
			if (res_lla)
			{
				res_lla[res][0] = llaA[0];
				res_lla[res][1] = llaA[1];
				res_lla[res][2] = llaA[2];
			}
			if (res_ecef)
			{
				res_ecef[res][0] = A_ECEF[0];
				res_ecef[res][1] = A_ECEF[1];
				res_ecef[res][2] = A_ECEF[2];
			}
			if (res_Distance) res_Distance[res] = D;
			if (res_Direction) res_Direction[res] = C * (rad?1:rad2deg);
			if (res_err) res_err[res] = err;
			++res;
		}
		//推进
		llaA[0] = Ap_lla[0];
		llaA[1] = Ap_lla[1];
		llaA[2] = Ap_lla[2];
		++cnt;
	} while (err>1 && res < maxResults);


	return res;
}
/*!
 * \brief ellips_range	椭球的最短测地线求取
 * \param llaA_in			A点经纬度
 * \param range				行进的距离
 * \param range				起始ENU方向
 * \param maxResults		结果的最大长度
 * \param resultSpan		结果的间隔为多个步长
 * \param res_lla			存储经纬度的结果
 * \param res_ecef			存储ECEF的结果
 * \param res_Distance		存储距离的结果
 * \param res_Direction		存储方向的结果
 * \param res_err			存储剩余距离的结果
 * \param rad				经纬度为弧度
 * \param latfirst			纬度在前([0])
 * \param enudelta			最大步长
 * \return					结果个数
 */
inline int ellips_range(
	const double llaA_in[/*3*/], //起始点经纬度高度
	const double range, //距离
	const double direction, //方向
	const int maxResults, //最大结果数
	const int resultSpan=1, //结果间隔
	double (*res_lla)[3] = nullptr, //结果经纬度
	double (*res_ecef)[3] = nullptr, //结果ECEF
	double (*res_Distance) = nullptr, //结果距离
	double (*res_Direction) = nullptr, //结果方向
	double (*res_err) = nullptr, //结果误差
	const bool rad = false, //是否为弧度
	const bool latfirst = true, //是否为纬度在前
	const double enudelta = 1000 //ENU增量
	)
{
	int res = 0; //结果计数
	const double H = llaA_in[2]; //起始点高度
	double llaA[] = {llaA_in[0],llaA_in[1],H}; //起始点经纬度高度
	double D = 0; //距离
	double A_ECEF[] = {0,0,0}; //ECEF坐标
	double err = 0; //误差
	int cnt = 0; //计数
	double C = direction * (rad?1:deg2rad); //方向
	do{
		double A_r = 0, A_d = 0; //A点半径和高度
		//计算A的ECEF
		lla2ecef(llaA,A_ECEF,&A_r,&A_d,rad,latfirst);
		//剩余距离
		const double dlft = range - D;

		if (dlft<=0)
			break;
		//确定本轮 delta
		const double delta = enudelta<dlft?(enudelta):(dlft);

		//计算当前方向
		const double C_enu[] = {sin(C),cos(C),0};
		const double Oz [] = {0,0,0};

		//单位矢量 Cs = C
		double Cs[] = {0,0,0};

		//A点ENU
		double R_A[3][3];
		enu_R_from_lla(llaA,R_A);
		enu2ecef(R_A,Oz,C_enu,Cs);

		//推进步进
		const double As[] = {A_ECEF[0],A_ECEF[1],A_ECEF[2]-A_d};
		const double omega = delta/(A_r+H);
		const double cos_omega = cos(omega);
		const double sin_omega = sin(omega);

		//求取 mu
		double mu[3] = {0,0,0};
		vec_cross(As,Cs,mu);
		const double las = vec_abs(As);
		mu[0] /= las;
		mu[1] /= las;
		mu[2] /= las;

		//mu x As
		double muCAs[3] = {0,0,0};
		vec_cross(mu,As,muCAs);

		//A's using 罗德里格旋转公式（Rodrigues' rotation formula）
		const double Ap_s[] = {
			cos_omega * As[0] + sin_omega * muCAs[0],
			cos_omega * As[1] + sin_omega * muCAs[1],
			cos_omega * As[2] + sin_omega * muCAs[2],
		};

		//计算下一位置 A'
		double Ap_ECEF[] = {Ap_s[0],Ap_s[1],Ap_s[2]+A_d};
		double Ap_lla[] = {0,0,0};
		//转为经纬度、修正高度
		double Ap_r = 0, Ap_d = 0;
		int iter = 0;
		ecef2lla(Ap_ECEF,Ap_lla,
				 &Ap_r,&Ap_d,
				 32,&iter,1e-10,
				 rad,latfirst);

		Ap_lla[2] = H;

		//距离累加
		D += (Ap_r + A_r+2*H)/2*omega;

		//更新 C 取得下一方向,以  A' 位置为正球
		const double Aps2 [] = {Ap_ECEF[0],Ap_ECEF[1],Ap_ECEF[2]-Ap_d};
		//求取 Cp 单位矢量
		double Cp[3] = {0,0,0};
		vec_cross(mu,Aps2,Cp);
		const double las2 = vec_abs(Aps2);
		Cp[0] /= las2;
		Cp[1] /= las2;
		Cp[2] /= las2;

		//求取 Cp的 ENU 坐标
		//A‘点ENU
		double R_Ap[3][3];
		double Cp_ENU[3] = {0,0,0};
		enu_R_from_lla(Ap_lla,R_Ap);
		ecef2enu(R_Ap,Oz,Cp,Cp_ENU);

		//求取更新 C
		const double aCp = atanCal(Cp_ENU[0],Cp_ENU[1]);

		//终止条件
		err = dlft;

		//输出中间路径
		if(res < maxResults && (cnt % resultSpan==0||err<=1))
		{
			if (res_lla)
			{
				res_lla[res][0] = llaA[0];
				res_lla[res][1] = llaA[1];
				res_lla[res][2] = llaA[2];
			}
			if (res_ecef)
			{
				res_ecef[res][0] = A_ECEF[0];
				res_ecef[res][1] = A_ECEF[1];
				res_ecef[res][2] = A_ECEF[2];
			}
			if (res_Distance) res_Distance[res] = D;
			if (res_Direction) res_Direction[res] = C * (rad?1:rad2deg);
			if (res_err) res_err[res] = err;
			++res;
		}
		//推进
		llaA[0] = Ap_lla[0];
		llaA[1] = Ap_lla[1];
		llaA[2] = Ap_lla[2];
		C = aCp;
		++cnt;
	} while (err>1 && res < maxResults);


	return res;
}

//! 4. 观测方位与俯仰遮盖
//////////////////////////////////////////////////////////////////////////////

/*!
 * \brief observe_az_el_range	  计算方位角、俯仰角
 * \param llaA_in	A点(观察者)
 * \param llaB_in	B点(被观察者)
 * \param paz		方位角
 * \param pel		俯仰角
 * \param prange	长度
 * \param ATMOSPHERIC_CORRECTION	大气折射矫正开关
 * \param rad		经纬度采用弧度
 * \param latfirst	纬度在第一维度[0]
 */
inline void observe_az_el_range(
	const double llaA_in[/*3*/],
	const double llaB_in[/*3*/],
	double * paz,
	double * pel,
	double * prange,
	bool ATMOSPHERIC_CORRECTION = false,
	const bool rad = false,
	const bool latfirst = true
	)
{
	double A_ECEF[] = {0,0,0}, B_ECEF[] = {0,0,0};
	//计算A的ECEF
	lla2ecef(llaA_in,A_ECEF,nullptr,nullptr,rad,latfirst);
	//计算B的ECEF
	lla2ecef(llaB_in,B_ECEF,nullptr,nullptr,rad,latfirst);
	//ENU
	double R_A[3][3];
	enu_R_from_lla(llaA_in,R_A,rad,latfirst);
	//计算B的ENU坐标
	double B_ENU[] = {0,0,0};
	ecef2enu(R_A,A_ECEF,B_ECEF,B_ENU);
	//计算当前Az
	if(paz)
		*paz = atanCal(B_ENU[0],B_ENU[1])*(rad?1:rad2deg);
	//计算El
	if(pel)
	{
		double el = asin(B_ENU[2]/vec_abs(B_ENU));
		//大气校正参考http://www.zeptomoby.com/satellites/
		if (ATMOSPHERIC_CORRECTION)
		{
			double saveEl = el;
			// Elevation correction for atmospheric refraction.
			// Reference:  Astronomical Algorithms by Jean Meeus, pp. 101-104
			// Note:  Correction is meaningless when apparent elevation is below horizon
			el += deg2rad*((1.02 /
							  tan(deg2rad*((rad2deg*el) + 10.3 /
																  ((rad2deg*el) + 5.11)))) / 60.0);
			if (el < 0.0)
			{
				// Reset to true elevation
				el = saveEl;
			}

			if (el > (w_pi / 2))
			{
				el = (w_pi / 2);
			}
		}
		*pel = el*(rad?1:rad2deg);
	}
	if (prange)
		*prange = vec_distance(A_ECEF,B_ECEF);
}

/*!
 * \brief observe_vis_judge_el	假设地球没有地形，是光滑的椭球，求取AB在仰角0下是不是可视。这个函数不需要迭代，速度较快
 * \param llaA_in		观测点的经纬度
 * \param llaB_in		被观测点经纬度
 * \param minEl			仰角约束，路径上所有地表位置观察A、B的仰角均要高于minEl
 * \param rad			经纬度采用弧度
 * \param latfirst		纬度在第一维度[0]
 * \return				true 是AB可视，false是不可视
 */
inline bool observe_vis_judge(
	const double llaA_in[/*3*/],
	const double llaB_in[/*3*/],
	const bool rad = false,
	const bool latfirst = true
	)
{
	double A_ECEF[] = {0,0,0}, B_ECEF[] = {0,0,0};
	//计算A的ECEF
	lla2ecef(llaA_in,A_ECEF,nullptr,nullptr,rad,latfirst);
	//计算B的ECEF
	lla2ecef(llaB_in,B_ECEF,nullptr,nullptr,rad,latfirst);
	//线段AB和椭球没有交点，就可以看到。

	const double AB []={B_ECEF[0]-A_ECEF[0],B_ECEF[1]-A_ECEF[1],B_ECEF[2]-A_ECEF[2]};
	const double absAB = vec_abs(AB);
	const double kAB[3] = {AB[0]/absAB,AB[1]/absAB,AB[2]/absAB};
	double interz[2][3] = {0,0,0,0,0,0}, intert[2]={-1,-1};
	int ri = ellipsoid_intersections(A_ECEF,kAB,0,interz,intert);

	bool vp = true;
	if(ri==0)
		return true;
	else if(ri==1)
		return false;
	else if (interz[0][0]>A_ECEF[0] &&interz[0][0]<B_ECEF[0]  )
		vp = false;
	else if (interz[1][0]>A_ECEF[0] &&interz[1][0]<B_ECEF[0]  )
		vp = false;
	else if (interz[0][1]>A_ECEF[1] &&interz[0][1]<B_ECEF[1]  )
		vp = false;
	else if (interz[1][1]>A_ECEF[1] &&interz[1][1]<B_ECEF[1]  )
		vp = false;
	else if (interz[0][2]>A_ECEF[2] &&interz[0][2]<B_ECEF[2]  )
		vp = false;
	else if (interz[1][2]>A_ECEF[2] &&interz[1][2]<B_ECEF[2]  )
		vp = false;

	return vp;
}

/*!
 * \brief observe_vis_judge_el	假设地球没有地形，是光滑的椭球，求取AB在仰角el下是不是可视。这个函数需要迭代，速度慢
 * \param llaA_in		观测点的经纬度
 * \param llaB_in		被观测点经纬度
 * \param minEl			仰角约束，路径上所有地表位置观察A、B的仰角均要高于minEl
 * \param rad			经纬度采用弧度
 * \param latfirst		纬度在第一维度[0]
 * \return				true 是AB可视，false是不可视
 */
inline bool observe_vis_judge_el(
	const double llaA_in[/*3*/],
	const double llaB_in[/*3*/],
	const double minEl = 0,
	const bool rad = false,
	const bool latfirst = true
	)
{
	if (minEl==0)
		return observe_vis_judge(llaA_in,llaB_in,rad,latfirst);
	double A_ECEF[] = {0,0,0}, B_ECEF[] = {0,0,0};
	//计算A的ECEF
	lla2ecef(llaA_in,A_ECEF,nullptr,nullptr,rad,latfirst);
	//计算B的ECEF
	lla2ecef(llaB_in,B_ECEF,nullptr,nullptr,rad,latfirst);
	const double AB []={B_ECEF[0]-A_ECEF[0],B_ECEF[1]-A_ECEF[1],B_ECEF[2]-A_ECEF[2]};
	double left_t = 0,right_t =1;
	double delta_t = (right_t - left_t)*0.01;
	double t = 0.5;
	bool vp = true;
	double abserr = 1000;
	while(vp && (abserr>1))
	{
		const double T_ECEF1 []={A_ECEF[0]+AB[0]*(t-delta_t),A_ECEF[1]+AB[1]*(t-delta_t),A_ECEF[2]+AB[2]*(t-delta_t)};
		double T_lla1[3] = {0,0,0};
		ecef2lla(T_ECEF1,T_lla1);
		const double T_ECEF2 []={A_ECEF[0]+AB[0]*(t+delta_t),A_ECEF[1]+AB[1]*(t+delta_t),A_ECEF[2]+AB[2]*(t+delta_t)};
		double T_lla2[3] = {0,0,0};
		ecef2lla(T_ECEF2,T_lla2);
		if (T_lla1[2] > T_lla2[2])
			left_t = t,abserr = T_lla1[2] - T_lla2[2];
		else
			right_t = t,abserr = T_lla2[2] - T_lla1[2];;
		delta_t = (right_t - left_t)*0.01;
		t = (left_t + right_t)/2;

		vp = T_lla1[2]>0 && T_lla2[2]>0;
	}
	//minEL
	if (vp)
	{
		const double deg_minEl = minEl * (rad?rad2deg:1);
		const double LLA_Adeg[] = {llaA_in[0]* (rad?rad2deg:1),llaA_in[1]* (rad?rad2deg:1),llaA_in[2]};
		const double LLA_Bdeg[] = {llaB_in[0]* (rad?rad2deg:1),llaB_in[1]* (rad?rad2deg:1),llaB_in[2]};
		double T_ECEF[]={A_ECEF[0]+AB[0]*t,A_ECEF[1]+AB[1]*t,A_ECEF[2]+AB[2]*t};
		double T_lla[3] = {0,0,0};
		ecef2lla(T_ECEF,T_lla);
		T_lla[2] = 0;
		double elA = 0;
		observe_az_el_range(T_lla,LLA_Adeg,nullptr,&elA,nullptr);
		if (deg_minEl >elA )
			vp = false;
		else
		{
			double elB = 0;
			observe_az_el_range(T_lla,LLA_Bdeg,nullptr,&elB,nullptr);
			if (deg_minEl >elB )
				vp = false;
		}
	}
	return vp;
}



/*!
 * \brief observe_dem_judge 根据数字高程DEM判断可视关系
 * \param llaA_in	观测点的经纬度
 * \param llaB_in   被观测点经纬度
 * \param dem		数字高程回调，给入纬度（度）、经度（度），返回高度（米）
 * \param dem_in_meters dem数据的分辨率（米）,将使用高于1/2分辨率的精度进行搜索测试
 * \param minEl			仰角约束，路径上所有地表位置观察A、B的仰角均要高于minEl
 * \param rad		经纬度采用弧度
 * \param latfirst	纬度在第一维度[0]
 * \return			true 是AB可视，false是不可视
 */
inline bool observe_dem_judge(
	const double llaA_in[/*3*/],
	const double llaB_in[/*3*/],
	std::function<double (double lat,double lon)> dem,
	const double dem_in_meters,
	const double minEl = 0,
	const bool rad = false,
	const bool latfirst = true
	)
{
	if (!dem)
		return observe_vis_judge(llaA_in,llaB_in,rad,latfirst);
	const double LLA_Adeg[] = {llaA_in[0]* (rad?rad2deg:1),llaA_in[1]* (rad?rad2deg:1),llaA_in[2]};
	const double LLA_Bdeg[] = {llaB_in[0]* (rad?rad2deg:1),llaB_in[1]* (rad?rad2deg:1),llaB_in[2]};
	const double deg_minEl = minEl * (rad?rad2deg:1);
	//计算AB在DEM上的长度
	double llaAE[] = {llaA_in[0],llaA_in[1],0};
	double llaBE[] = {llaB_in[0],llaB_in[1],0};
	double A_ECEF_E[] = {0,0,0}, B_ECEF_E[] = {0,0,0};
	lla2ecef(llaAE,A_ECEF_E,nullptr,nullptr,rad,latfirst);
	lla2ecef(llaBE,B_ECEF_E,nullptr,nullptr,rad,latfirst);
	const double ABE []={B_ECEF_E[0]-A_ECEF_E[0],B_ECEF_E[1]-A_ECEF_E[1],B_ECEF_E[2]-A_ECEF_E[2]};
	const double len_ab = vec_abs(ABE);
	//分辨率到归一化步长
	const double dt = dem_in_meters/len_ab/2;

	double A_ECEF[] = {0,0,0}, B_ECEF[] = {0,0,0};
	//计算A的ECEF
	lla2ecef(llaA_in,A_ECEF,nullptr,nullptr,rad,latfirst);
	//计算B的ECEF
	lla2ecef(llaB_in,B_ECEF,nullptr,nullptr,rad,latfirst);

	const double AB []={B_ECEF[0]-A_ECEF[0],B_ECEF[1]-A_ECEF[1],B_ECEF[2]-A_ECEF[2]};
	bool vp = true;
	for (double t = 0; t<=1 && vp; t+=dt)
	{
		const double T_ECEF []={A_ECEF[0]+AB[0]*t,A_ECEF[1]+AB[1]*t,A_ECEF[2]+AB[2]*t};
		double T_lla[3] = {0,0,0};
		ecef2lla(T_ECEF,T_lla);
		const double h = T_lla[2];
		const double H = dem(T_lla[0],T_lla[1]);
		vp = h > H;
		//minEL
		if (minEl>0 && vp)
		{
			T_lla[2] = H;
			double elA = 0;
			observe_az_el_range(T_lla,LLA_Adeg,nullptr,&elA,nullptr);
			if (deg_minEl >elA )
				vp = false;
			else
			{
				double elB = 0;
				observe_az_el_range(T_lla,LLA_Bdeg,nullptr,&elB,nullptr);
				if (deg_minEl >elB )
					vp = false;
			}
		}
	}
	return vp;
}

//!5. Heading Pitch Roll 坐标变换
//////////////////////////////////////////////////////////////////////////////


/*!
 * \brief hpr_T_from_hpr HPR 的三轴在ENU下的向量
 * \param hpr: Heading=h Pitch=p, Roll=r，小写的hpr是角度
 * \param T	: ENU下的HPR三轴向量。在旋转为0时， E=R N=H U=P。大写的HPR是笛卡尔坐标
 * \param rad 弧度true/度 false
 */
inline void hpr_T_in_enu(
	const double hpr[/*3*/],
	double T[/*3*/][3],
	const bool rad = false	)
{
	const double h = (rad ? (hpr[0]) : (hpr[0] * deg2rad));
	const double p = (rad ? (hpr[1]) : (hpr[1] * deg2rad));
	const double r = (rad ? (hpr[2]) : (hpr[2] * deg2rad));

	const double cos_h = cos(h), sin_h = sin(h);
	const double cos_p = cos(p), sin_p = sin(p);
	const double cos_r = cos(r), sin_r = sin(r);

	T[0][0] = sin_h * cos_p ;
	T[0][1] = cos_h * cos_p;
	T[0][2] = sin_p ;

	T[1][0] = -sin_h * cos_r * sin_p
			  + sin_r * cos_h ;
	T[1][1] = -cos_h * cos_r * sin_p
			  - sin_r * sin_h ;
	T[1][2] = cos_p * cos_r;

	T[2][0] = cos_h * cos_r + sin_p * sin_h * sin_r;
	T[2][1] = -sin_h * cos_r + cos_h * sin_p * sin_r;
	T[2][2] = - cos_p * sin_r;
}
/*!
 * \brief enu2HPR enu 笛卡尔坐标到 HPR 笛卡尔坐标。注意，大写HPR是坐标不是角度。
 * \param T		HPR 的三轴在ENU下的向量，由hpr_T_in_enu生成
 * \param A_enu	enu坐标矢量
 * \param A_HPR	HPR坐标矢量
 */
inline void enu2HPR(
	const double T[/*3*/][3],
	const double A_enu[/*3*/],
	double A_HPR[/*3*/])
{
	A_HPR[0] = T[0][0] * A_enu[0] + T[0][1] * A_enu[1] + T[0][2] * A_enu[2];
	A_HPR[1] = T[1][0] * A_enu[0] + T[1][1] * A_enu[1] + T[1][2] * A_enu[2];
	A_HPR[2] = T[2][0] * A_enu[0] + T[2][1] * A_enu[1] + T[2][2] * A_enu[2];
}

/*!
 * \brief HPR2enu  HPR 笛卡尔坐标到 enu笛卡尔坐标。注意，大写HPR是坐标不是角度。
 * \param T		HPR 的三轴在ENU下的向量，由hpr_T_in_enu生成
 * \param A_HPR	HPR坐标矢量
 * \param A_enu	enu坐标矢量
 */
inline void HPR2enu(
	const double T[/*3*/][3],
	const double A_HPR[/*3*/],
	double A_enu[/*3*/])
{
	A_enu[0] = T[0][0] * A_HPR[0] + T[1][0] * A_HPR[1] + T[2][0] * A_HPR[2];
	A_enu[1] = T[0][1] * A_HPR[0] + T[1][1] * A_HPR[1] + T[2][1] * A_HPR[2];
	A_enu[2] = T[0][2] * A_HPR[0] + T[1][2] * A_HPR[1] + T[2][2] * A_HPR[2];
}


/*!
 * \brief contour_distance 计算到空间位置LLA坐标 T 距离为D的地面位置集合。
 * \param lla_torch		全向光源的位置
 * \param queryD		距离等值线的距离。D=0会返回最短距离， D超过最大距离，则返回最大距离。
 * \param earth_H		地表高度
 * \param vec_beta		给定的HPR beta，正北是0，顺时针递增
 * \param vec_lat		存储纬度的向量
 * \param vec_lon		存储经度的向量
 * \param vec_D			存储距离的向量
 * \param derrMeter		迭代误差，默认1mm
 * \param rad			弧度true/度 false
 * \param latfirst		纬度经度高度 true/经度 纬度 高度 false
 */
inline void contour_distance(const double lla_torch [3],
					  const double queryD,
					  const double earth_H,
					  const std::vector<double> vec_beta,
					  std::vector<double> * vec_lat,
					  std::vector<double> * vec_lon,
					  std::vector<double> * vec_D,
					  const double derrMeter = 1e-3,
					  const bool rad = false,
					  const bool latfirst = true)
{
	const size_t points = vec_beta.size();
	if (points<=0)
		return;
	//光源的姿态角度, 全向光源指向地面
	const double hpr_torch [3] = {0,-90,0};
	double T_torch[3][3] = {0,0,0,0,0,0,0,0,0};
	hpr_T_in_enu(hpr_torch,T_torch);
	//光源 ECEF
	double ecef_torch[3] = {0,0,0};
	lla2ecef(lla_torch,ecef_torch,nullptr,nullptr,rad,latfirst);
	//光源ENU
	double R_torch[3][3] = {0,0,0,0,0,0,0,0,0};
	enu_R_from_lla(lla_torch,R_torch,rad,latfirst);

	double D = queryD;
	//最短距离、投影位置
	double
		minD = lla_torch [2] - earth_H,
		minAlpha=0,
		minLLA[3]={0,0,0}
	;

	if (D < minD)
		D = minD;

	if (minD<0)
		return;
	else
	{
		const double kH_enu[3] = {T_torch[0][0],T_torch[0][1],T_torch[0][2]};
		double kH_ecef[3] = {0,0,0};
		enu2ecef(R_torch,kH_enu,kH_ecef);
		double intersecs_ecef[2][3], intersecs_t[2];
		int c_inter = ellipsoid_intersections(ecef_torch,kH_ecef,earth_H,intersecs_ecef,intersecs_t);
		if (c_inter)
			ecef2lla(intersecs_ecef[0],minLLA,0,0,32,0,1e-10,rad,latfirst);
		else
			return;
	}

	//最大距离、切点位置
	std::vector<double> maxD,maxAlpha,maxLLA01,maxLLA02;

	for (size_t pti = 0; pti < points; ++pti)
	{
		const double beta = vec_beta[pti];
		double leftAlpha = 0, rightAlpha = w_pi/2;
		double alpha = (leftAlpha + rightAlpha)/2;
		bool finished = false;
		double intersecs_ecef[2][3], intersecs_t[2];
		while (!finished)
		{
			//beta为0时的HPR光矢
			double P[3] = {cos(alpha),sin(alpha),0};
			double Hz[3] = {1,0,0},Q[3] = {0,0,0};
			//旋转
			vec_rotation(P,Hz,beta,Q);
			//HPR-->enu
			double Q_enu[3] = {0,0,0};
			HPR2enu(T_torch,Q,Q_enu);
			//enu-->ecef
			double Q_ecef[3] = {0,0,0};
			enu2ecef(R_torch,Q_enu,Q_ecef);
			//求交点
			const int c_inter = ellipsoid_intersections(ecef_torch,Q_ecef,earth_H,intersecs_ecef,intersecs_t);
			if (!c_inter)
			{
				rightAlpha = alpha;
			}
			else if (c_inter==2)
			{
				leftAlpha = alpha;
			}
			else
				finished = true;
			alpha = (leftAlpha + rightAlpha)/2;
			if (alpha==leftAlpha || alpha==rightAlpha)
				finished = true;
		}
		double lla_i[3]={0,0,0};
		ecef2lla(intersecs_ecef[0],lla_i,0,0,32,0,1e-10,rad,latfirst);

		maxD.push_back(intersecs_t[0]);
		maxAlpha.push_back(alpha);
		maxLLA01.push_back(lla_i[0]);
		maxLLA02.push_back(lla_i[1]);
	}
	//二分计算
	for (size_t pti = 0; pti < points; ++pti)
	{
		if (D<=minD)
		{
			vec_lat->push_back(latfirst? minLLA[0]:minLLA[1]);
			vec_lon->push_back(latfirst? minLLA[1]:minLLA[0]);
			vec_D->push_back(minD);
			continue;
		}
		else if (D>=maxD[pti])
		{
			vec_lat->push_back(latfirst? maxLLA01[pti]:maxLLA02[pti]);
			vec_lon->push_back(latfirst? maxLLA02[pti]:maxLLA01[pti]);
			vec_D->push_back(maxD[pti]);
			continue;
		}
		bool ok = false;
		const double beta = vec_beta[pti];
		double leftAlpha = minAlpha, rightAlpha = maxAlpha[pti];
		double alpha = (leftAlpha + rightAlpha)/2;
		double leftD = minD, rightD = maxD[pti];
		//求取距离
		double intersecs_ecef[2][3], intersecs_t[2];
		while (!ok)
		{
			double P[3] = {cos(alpha),sin(alpha),0};
			double Hz[3] = {1,0,0},Q[3] = {0,0,0};
			//旋转
			vec_rotation(P,Hz,beta,Q);
			//HPR-->enu
			double Q_enu[3] = {0,0,0};
			HPR2enu(T_torch,Q,Q_enu);
			//enu-->ecef
			double Q_ecef[3] = {0,0,0};
			enu2ecef(R_torch,Q_enu,Q_ecef);
			//求交点
			const int c_inter = ellipsoid_intersections(ecef_torch,Q_ecef,earth_H,intersecs_ecef,intersecs_t);
			if (!c_inter)
				break;
			if (intersecs_t[0] >D)
			{
				rightAlpha = alpha;
				rightD = intersecs_t[0];
			}
			else
			{
				leftAlpha = alpha;
				leftD = intersecs_t[0];
			}
			alpha = (leftAlpha + rightAlpha)/2;
			if (leftD + derrMeter >=rightD)
				ok = true;
			else if (alpha==leftAlpha || alpha==rightAlpha)
				ok = true;
		}
		if (ok)
		{
			double lla_i[3]={0,0,0};
			ecef2lla(intersecs_ecef[0],lla_i,0,0,32,0,1e-10,rad,latfirst);
			vec_lat->push_back(latfirst? lla_i[0]:lla_i[1]);
			vec_lon->push_back(latfirst? lla_i[1]:lla_i[0]);
			vec_D->push_back(intersecs_t[0]);
		}
		else
		{
			vec_lat->push_back(0);
			vec_lon->push_back(0);
			vec_D->push_back(-1);
		}
	}

}


/*!
 * \brief contour_elevation 计算到空间位置LLA坐标 T 仰角为el的地面位置集合。
 * \param lla_torch		全向光源的位置
 * \param queryEl		角度等值线的仰角。El=90会返回最短距离， El=0返回切面最大距离。
 * \param earth_H		地表高度
 * \param vec_beta		给定的HPR beta，正北是0，顺时针递增
 * \param vec_lat		存储纬度的向量
 * \param vec_lon		存储经度的向量
 * \param vec_D			存储距离的向量
 * \param vec_Az		存储方位角的向量
 * \param vec_El		存储俯仰角的向量
 * \param ATMOSPHERIC_CORRECTION		大气折射矫正
 * \param derrMeter		迭代误差，默认1mm
 * \param rad			弧度true/度 false
 * \param latfirst		纬度经度高度 true/经度 纬度 高度 false
 */
inline void contour_elevation(const double lla_torch [3],
					  const double queryEl,
					  const double earth_H,
					  const std::vector<double> vec_beta,
					  std::vector<double> * vec_lat,
					  std::vector<double> * vec_lon,
					  std::vector<double> * vec_D,
					  std::vector<double> * vec_Az,
					  std::vector<double> * vec_El,
					  const bool ATMOSPHERIC_CORRECTION = false,
					  const double derrMeter = 1e-3,
					  const bool rad = false,
					  const bool latfirst = true)
{
	const size_t points = vec_beta.size();
	if (points<=0)
		return;
	//光源的姿态角度, 全向光源指向地面
	const double hpr_torch [3] = {0,-90,0};
	double T_torch[3][3] = {0,0,0,0,0,0,0,0,0};
	hpr_T_in_enu(hpr_torch,T_torch);
	//光源 ECEF
	double ecef_torch[3] = {0,0,0};
	lla2ecef(lla_torch,ecef_torch,nullptr,nullptr,rad,latfirst);
	//光源ENU
	double R_torch[3][3] = {0,0,0,0,0,0,0,0,0};
	enu_R_from_lla(lla_torch,R_torch,rad,latfirst);

	double dEl = queryEl ;
	//最高仰角、投影位置
	double
		maxEL = rad?(w_pi/2):90,
		maxEL_minD = lla_torch [2] - earth_H,
		maxEl_minAlpha=0,
		maxEl_LLA[3]={0,0,0}
	;

	if (dEl >= maxEL)
		dEl = maxEL;

	if (maxEL_minD>=0)
	{
		const double kH_enu[3] = {T_torch[0][0],T_torch[0][1],T_torch[0][2]};
		double kH_ecef[3] = {0,0,0};
		enu2ecef(R_torch,kH_enu,kH_ecef);
		double intersecs_ecef[2][3], intersecs_t[2];
		int c_inter = ellipsoid_intersections(ecef_torch,kH_ecef,earth_H,intersecs_ecef,intersecs_t);
		if (c_inter)
			ecef2lla(intersecs_ecef[0],maxEl_LLA,0,0,32,0,1e-10,rad,latfirst);
		else
			return;
	}
	else
		return;

	//最大距离、切点位置
	const double minEl = 0;
	if (dEl <= minEl)
		dEl = minEl;
	std::vector<double> minEl_maxD,minEl_maxAlpha,minEl_LLA0,minEl_LLA1;

	for (size_t pti = 0; pti < points; ++pti)
	{
		const double beta = vec_beta[pti];
		double leftAlpha = 0, rightAlpha = w_pi/2;
		double alpha = (leftAlpha + rightAlpha)/2;
		bool finished = false;
		double intersecs_ecef[2][3], intersecs_t[2];
		while (!finished)
		{
			//beta为0时的HPR光矢
			double P[3] = {cos(alpha),sin(alpha),0};
			double Hz[3] = {1,0,0},Q[3] = {0,0,0};
			//旋转
			vec_rotation(P,Hz,beta,Q);
			//HPR-->enu
			double Q_enu[3] = {0,0,0};
			HPR2enu(T_torch,Q,Q_enu);
			//enu-->ecef
			double Q_ecef[3] = {0,0,0};
			enu2ecef(R_torch,Q_enu,Q_ecef);
			//求交点
			const int c_inter = ellipsoid_intersections(ecef_torch,Q_ecef,earth_H,intersecs_ecef,intersecs_t);
			if (!c_inter)
			{
				rightAlpha = alpha;
			}
			else if (c_inter==2)
			{
				leftAlpha = alpha;
			}
			else
				finished = true;
			alpha = (leftAlpha + rightAlpha)/2;
			if (alpha==leftAlpha || alpha==rightAlpha)
				finished = true;
		}
		double lla_i[3]={0,0,0};
		ecef2lla(intersecs_ecef[0],lla_i,0,0,32,0,1e-10,rad,latfirst);

		minEl_maxD.push_back(intersecs_t[0]);
		minEl_maxAlpha.push_back(alpha);
		minEl_LLA0.push_back(lla_i[0]);
		minEl_LLA1.push_back(lla_i[1]);
	}
	//二分计算
	for (size_t pti = 0; pti < points; ++pti)
	{
		if (dEl>=maxEL)
		{
			vec_lat->push_back(latfirst? maxEl_LLA[0]:maxEl_LLA[1]);
			vec_lon->push_back(latfirst? maxEl_LLA[1]:maxEl_LLA[0]);
			vec_D->push_back(maxEL_minD);
			vec_El->push_back(rad?(w_pi/2):90);
			vec_Az->push_back(0);
			continue;
		}
		else if (dEl<=minEl)
		{
			vec_lat->push_back(latfirst? minEl_LLA0[pti]:minEl_LLA1[pti]);
			vec_lon->push_back(latfirst? minEl_LLA1[pti]:minEl_LLA0[pti]);
			vec_D->push_back(minEl_maxD[pti]);
			double lla_in[3]={minEl_LLA0[pti],minEl_LLA1[pti],earth_H};
			double paz = 0,pel = 0,pd = 0;
			observe_az_el_range(lla_in,lla_torch,&paz,&pel,&pd,ATMOSPHERIC_CORRECTION,rad,latfirst);
			vec_El->push_back(pel);
			vec_Az->push_back(paz);
			continue;
		}
		bool ok = false;
		const double beta = vec_beta[pti];
		double leftAlpha = maxEl_minAlpha, rightAlpha = minEl_maxAlpha[pti];
		double alpha = (leftAlpha + rightAlpha)/2;
		double leftEl = maxEL, rightEl = minEl;
		double leftD = maxEL_minD , rightD = minEl_maxD[pti];
		//求取距离
		double intersecs_ecef[2][3], intersecs_t[2];
		//观察
		double lla_i[3]={0,0,0};
		double paz = 0,pel = 0,pd = 0;
		while (!ok)
		{
			double P[3] = {cos(alpha),sin(alpha),0};
			double Hz[3] = {1,0,0},Q[3] = {0,0,0};
			//旋转
			vec_rotation(P,Hz,beta,Q);
			//HPR-->enu
			double Q_enu[3] = {0,0,0};
			HPR2enu(T_torch,Q,Q_enu);
			//enu-->ecef
			double Q_ecef[3] = {0,0,0};
			enu2ecef(R_torch,Q_enu,Q_ecef);
			//求交点
			const int c_inter = ellipsoid_intersections(ecef_torch,Q_ecef,earth_H,intersecs_ecef,intersecs_t);
			if (!c_inter)
				break;

			ecef2lla(intersecs_ecef[0],lla_i,0,0,32,0,1e-10,rad,latfirst);
			observe_az_el_range(lla_i,lla_torch,&paz,&pel,&pd,ATMOSPHERIC_CORRECTION,rad,latfirst);

			if (pel < dEl)
			{
				rightAlpha = alpha;
				rightEl = pel;
				rightD = intersecs_t[0];
			}
			else
			{
				leftAlpha = alpha;
				leftEl = pel;
				leftD = intersecs_t[0];
			}
			alpha = (leftAlpha + rightAlpha)/2;

			if (leftD + derrMeter >=rightD)
				ok = true;
			else if (alpha==leftAlpha || alpha==rightAlpha)
				ok = true;
		}
		if (ok)
		{
			vec_lat->push_back(latfirst? lla_i[0]:lla_i[1]);
			vec_lon->push_back(latfirst? lla_i[1]:lla_i[0]);
			vec_D->push_back(intersecs_t[0]);
			vec_El->push_back(pel);
			vec_Az->push_back(paz);
		}
		else
		{
			vec_lat->push_back(0);
			vec_lon->push_back(0);
			vec_D->push_back(-1);
			vec_El->push_back(0);
			vec_Az->push_back(0);
		}
	}

}

} // namespace CES_GEOCALC
